#include <map>
#include <string>
#include <iostream>
#include <iostream>
#include <vector>
#include <eigen3/Eigen/Dense>
#include <stdexcept>
#include <boost/timer.hpp>
#include <boost/progress.hpp>
#include <iostream> // std::cout
#include <functional> // std::bind
#include<fstream>
#include <opencv2/opencv.hpp>
// 如果导入了opencv,需要按照如下命令跑代码
/*

cmake .
make
./demo

*/


int main(void)
{
  // int data_ptr[9];
  // memset(data_ptr, 1, sizeof(data_ptr));
  // //  std::cout << "data_ptr:" << *data_ptr <<  std::endl;
  // std::cout << "data_ptr:" << std::endl;
  // for (int i=0; i<9; ++i)
  // {
  //     std::cout << data_ptr[i] << std::endl;
  // }
  // cv::Mat bev_image(3, 3, CV_8UC1, data_ptr);
  // bev_image = bev_image * 3.2f;
  // std::cout << "bev_image:" << bev_image  << std::endl;
  // // bev_image.at<int>(0,0) = 2;
  // // std::cout << "bev_image:" << bev_image << std::endl;
  // // std::cout << "data_ptr:" << std::endl;
  // //   for (int i=0; i<9; ++i)
  // // {
  // //     std::cout << data_ptr[i] << std::endl;
  // // }
  // // std::cout << "aaa" << std::endl;
  std::vector<float> matrix = {1.f, 2.f, 3.f, 4.f, 5.f, 6.f};
  cv::Mat centric_coords(2, 3, CV_32F, matrix.data());
  std::cout << centric_coords << std::endl;
  float *coords_x_pre_ptr =
          reinterpret_cast<float *>(centric_coords.ptr(0));
  float *coords_y_pre_ptr =
      reinterpret_cast<float *>(centric_coords.ptr(1));
  float *coords_y_ptr = reinterpret_cast<float *>(centric_coords.ptr(1));
  float *coords_x_ptr = reinterpret_cast<float *>(centric_coords.ptr(0));
  int traj_len_ = 2;
  int scale_count = 0;
  int coord_idx = 1;
  float scale = 0.0f;
  for (; coord_idx < traj_len_ + 1; ++coord_idx) {
    // his_vx = centric_vx + 0.5f * centric_ax *
    //          (traj_pred_timestep_ * coord_idx);
    // scale = his_vx * traj_pred_timestep_ * coord_idx /
    //         (*coords_x_ptr + FLT_MIN);
    // if (scale >= 1.0f || scale < 0.0f) {
    //   scale = 1.0f;
    // }
    scale = 2.0f;
    float last_coord_x = coord_idx != 1 ? *coords_x_pre_ptr : 0.0f;
    // if (*coords_x_ptr * scale > last_coord_x) {
      if (coord_idx == traj_len_) {
        *coords_x_ptr = *coords_x_ptr * scale;
        *coords_y_ptr = *coords_y_ptr;
      } else {
        *coords_x_ptr++ = *coords_x_ptr * scale;
        std::cout << "coords_y_ptr" << coords_y_ptr << std::endl;
        // *coords_y_ptr++ = *coords_y_ptr;
        std::cout << "coords_y_ptr" << *coords_y_ptr++ << std::endl;
        std::cout << "coords_y_ptr" << *coords_y_ptr << std::endl;
      }
    //   if (scale != 1.0f) {
    //     scale_count += 1;
    //   }
    // } else {
    //   break;
    // }
  
    // if (coord_idx != 1) {
    //   *coords_x_pre_ptr++ = *coords_x_pre_ptr;
    //   *coords_y_pre_ptr++ = *coords_y_pre_ptr;
    // }
    std::cout << "coords_x_ptr:" << *coords_x_ptr << " coords_x_pre_ptr:" << *coords_x_pre_ptr << "\n";
    // std::cout << "coord_idx:" << coord_idx << centric_coords << std::endl;  
  }
  std::cout << "coord_idx" << coord_idx;
  // for (; coord_idx < traj_len_ + 1; ++coord_idx) {
  //       *coords_x_ptr++ = *coords_x_pre_ptr;
  //       *coords_y_ptr++ = *coords_y_pre_ptr;
  // }
  std::cout << centric_coords << std::endl;
  float std_weight_position = 1. / 20;
  float std_weight_velocity = 1. / 160;
  Eigen::Matrix<float, 4, 1> mean;
  mean << 1, 2, 3, 4;
  Eigen::VectorXf aa(4);
  aa << std_weight_position * mean[3],
          std_weight_position * mean[3],
          1e-2,
          std_weight_position * mean[3];
  std::cout << Eigen::MatrixXf(aa.array().pow(2).matrix().asDiagonal()) << std::endl;
  Eigen::Matrix<float, 4, 8> motionMat;
  motionMat.setIdentity(4, 8);
  // for (int i = 0; i < 4; ++i) {
  //   motionMat(i, 4 + i) = 1;
  // }
  std::cout << motionMat << std::endl;
  return 0;
}